package algo;

import geom.LineSegment;
import io.HandleFiles;

import java.io.File;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.HashSet;
import java.util.Set;

import com.vividsolutions.jts.geom.Coordinate;

import entity.TLocation;

public class MyProcessor {

	static double tolerance = 7.5E-5;
	static Simplification s;
	static String pPath = "E:\\Research\\au\\swarm\\data\\data_routes\\team1_car01_no_home_1_done_36_26.txt";
	static String qPath = "E:\\Research\\au\\swarm\\data\\data_routes\\team1_car01_no_home_1_done_36_23.txt";

	public static void main(String[] args) throws Exception {
		s = new Simplification();
		ArrayList<String[]> pData = HandleFiles.readRouteFile(new File(pPath));
		ArrayList<String[]> qData = HandleFiles.readRouteFile(new File(qPath));

		TLocation[] loc = new TLocation[pData.size()];
		int i = 0;
		for (String[] arr : pData) {
			// System.out.println(arr[0] + ", " + arr[1] + "," + arr[2]);
			loc[i++] = new TLocation(Double.parseDouble(arr[0]),
					Double.parseDouble(arr[1]), Double.parseDouble(arr[2]));
		}

		Coordinate[] pcurve = new Coordinate[loc.length];
		// Coordinate[] qcurve = new Coordinate[loc.length];
		for (i = 0; i < loc.length; i++) {
			pcurve[i] = new Coordinate(loc[i].locx, loc[i].locy);
		}

		loc = null;
		loc = new TLocation[qData.size()];
		i = 0;
		for (String[] arr : qData) {
			// System.out.println(arr[0] + ", " + arr[1] + "," + arr[2]);
			loc[i++] = new TLocation(Double.parseDouble(arr[0]),
					Double.parseDouble(arr[1]), Double.parseDouble(arr[2]));
		}
		Coordinate[] qcurve = new Coordinate[loc.length];
		// Coordinate[] qcurve = new Coordinate[loc.length];
		for (i = 0; i < loc.length; i++) {
			qcurve[i] = new Coordinate(loc[i].locx, loc[i].locy);
		}

		// do line simp

		// System.out.println("before simp: " + pcurve.length);
		// System.out.println(pcurve[0]+" "+pcurve[2]);
		// System.out.println("dist: "+ pcurve[0].distance(pcurve[2]));

		// simplify pcurve
		s.setRouteCoords(pcurve);
		Integer[] indices = s.simplifyArr(0, tolerance);

		TLocation[] simplePCurve = new TLocation[indices.length];
		for (i = 0; i < indices.length; i++) {
			simplePCurve[i] = loc[indices[i]];
		}
		// simply qcurve
		s.setRouteCoords(qcurve);
		indices = s.simplifyArr(0, tolerance);
		TLocation[] simpleQCurve = new TLocation[indices.length];
		for (i = 0; i < indices.length; i++) {
			simpleQCurve[i] = loc[indices[i]];
		}

		// compute velocity
		System.out.println("after simp, pcurve: " + simplePCurve.length);
		System.out.println("after simp, qcurve: " + simpleQCurve.length);

		// compute integral
		double integDist = distance(simplePCurve, simpleQCurve);
		System.out.println("the distance between p and q is"+integDist);
	}

	/**
	 * TODO: verify this method
	 * 
	 * Compute the integral distance between two curves Assuming two curves both
	 * start from 0
	 * 
	 * @param simplePCurve
	 * @param simpleQCurve
	 * @return integral distance between two curves
	 */
	public static double distance(TLocation[] simplePCurve,
			TLocation[] simpleQCurve) {
		double totalDist=0;
		int l1 = simplePCurve.length;
		int l2 = simpleQCurve.length;
		LineSegment pls;
		LineSegment qls;
		TLocation ptl1, ptl2, qtl1, qtl2;
		int qstop = 0;
		
		
		if (l1 < l2) {
			for (int i = 0; i < l1 - 1; i++) {
				ptl1 = simplePCurve[i];
				ptl2 = simplePCurve[i + 1];

				pls = new LineSegment(ptl1.locx, ptl1.locy, ptl1.time,
						ptl2.locx, ptl2.locy, ptl2.time);
				
				qtl1 = simpleQCurve[qstop];
				qtl2 = simpleQCurve[qstop + 1];
				qls = new LineSegment(qtl1.locx, qtl1.locy, qtl1.time,
						qtl2.locx, qtl2.locy, qtl2.time);
				if (pls.isOverlap(qls)){
					totalDist += pls.distance(qls);
				}else{
					if(ptl2.time < qtl1.time){
						continue;
					}else{
						i--;
						qstop++;
					}
				}
			}
		}
		return totalDist;
	}
}
